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SUMMARY 


'  ^ 

The  dynamic  equations  of  multi-body  systems  in  the  form  of  open 
chains  are  derived  by  applying  the  principles  of  linear  and  angular  momen¬ 
tum  to  each  individual  member  in  the  chain.  This  results  in  the  appearance 
of  constraint  forces  and  torques  in  the  dynamic  equations.  Using  more  or 
less  classical  approach  these  unknown  forces  and  torques  can  be  eliminated. 
Another  approach  is  to  approximate  these  forces  by  elastic  and  viscous 
forces  by  allowing  small  violations  of  the  constraints.  The  well-known 
elimination  procedure  leads  to  a  small  densely  coupled  system  of  equations 
while  the  lesser-known  procedure  of  approximating  the  constraint  forces 
and  torques  yields  a  large  but  less  densely  coupled  system.  Both  these 
procedures  are  first  explained  in  the  context  of  a  single  rigid  body  and  then 
applied  to  a  system  of  rigid  bodies  in  an  open  chain  where  each  body  is 
coupled  directly  to  at  most  two  neighbours. 


RESUME 

Les  equations  dynamiques  d’un  systeme  de  plusieurs  corps  disposes  en 
chaines  ouvertes  sont  derivees  en  appliquant  les  principes  de  la  quantite  de 
mouvement  et  du  moment  cinetique  a  chaque  membre  de  la  chaine.  II  en 
resulte  que  des  contraintes  de  force  et  de  couple  se  degagent  des  equations 
dynamiques.  Par  une  demarche  plus  ou  moins  classique,  on  peut  eliminer 
ces  forces  et  ces  couples  inconnus.  Une  autre  solution  consiste  a  remplacer 
ces  forces  par  des  forces  d ’elasticity  et  de  viscosite  approximatives,  en 
violant  quelque  peu  les  contraintes.  La  methode  classique  d’elimination 
donne  un  petit  systeme  d’equations  fortement  couplees,  tandis  que  la 
methode  moins  repandue  d’approximation  des  contraintes  de  force  et  de 
couple  produit  un  grand  systeme  d’equations  moins  fortement  couplees. 
Les  deux  methodes  sont  appliquees  d’abord  a  l’etude  d’un  corps  rigide 
unique,  puis  a  l’etude  d’un  systeme  de  corps  rigides  disposes  en  une  chaine 
ouverte  ou  chaque  corps  est  couple  directement  a  au  plus  deux  corps  voisins. 
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DYNAMICS  OF  MULTI-BODY  SYSTEMS 


1.0  INTRODUCTION 

During  the  last  two  decades  there  has  been  a  considerable  interest  in  the  study  of  multi-body 
systems,  that  is  systems  consisting  of  a  finite  number  of  interconnected  rigid  bodies.  Typical  examples 
are  manipulators,  linkages  in  machines  and  mechanism  of  human  body  provided  each  part  is  considered 
as  rigid.  Although  the  general  principles  of  obtaining  the  equations  of  motion  of  such  systems  have  been 
known  since  the  days  of  Euler  (1707-1783)  and  Lagrange  (1726-1813)  yet  there  is  a  need  for  finding 
general  and  computer  oriented  methods  as  the  formalism  suitable  for  analytical  purposes  may  not  be 
convenient  for  computer  simulation.  Most  of  the  recent  work  (Refs.  1-3)  is  therefore  devoted  to  ob¬ 
taining  methods  that  are  efficient  and  general  enough  to  be  applicable  to  a  wide  variety  of  multi-body 
systems  with  a  minimum  amount  of  preparatory  work. 

The  purpose  of  this  report  is  to  explain  in  as  simple  a  way  as  possible  some  of  the  methods  of 
deriving  the  equations  of  motion  of  multi-body  systems.  To  accomplish  this  we  assume  that  we  are 
given  a  system  of  n  rigid  bodies  connected  together  in  an  open  chain  such  that  each  body  is  coupled 
directly  to  at  most  two  neighbours  as  shown  in  Figure  1. 


The  bodies  are  attached  to  each  other  either  by  ball-and-socket  joint,  universal  joint  or  by  pin  joint*. 

The  principles  of  linear  and  angular  momentum  (Newton’s  and  Eulers’s  law)  applied  to  each 
individual  body  in  the  chain  provide  a  simple  way  of  writing  the  equations  of  motion  of  a  multi-body 
system.  However,  this  results  in  the  appearance  of  unknown  constraint  forces  and  torques.  There  are 
now  essentially  two  approaches  for  removing  them.  In  the  classical  approach  these  forces  and  torques 
are  eliminated  from  the  6n  second-order  differential  equations  resulting  in  at  most  3n+3  equations  cor¬ 
responding  to  the  number  of  degrees  of  freedom.  The  equations  so  obtained  are  complicated  and 
densely  coupled.  In  the  other  approach  each  body  is  kept  as  a  free  body  and  the  constraint  forces  and 
torques  are  approximated  by  elastic  and  viscous  forces  by  allowing  small  violations  of  the  constraints. 
(They  can  also  be  approximated  numerically  by  solving  the  equations  of  motion  and  constraint  equa¬ 
tions  simultaneously.)  The  number  of  equations  to  be  solved  in  this  approach  is  more  than  that  solved 
in  the  first  approach.  However,  they  are  simple  and  less  densely  coupled  and  may  be  as  convenient  to 
solve  on  the  computer  as  the  smaller  number  of  more  complicated  equations. 

Apart  from  the  introductory  section  the  report  is  divided  into  four  sections.  In  Section  2  we 
present  the  equations  of  motion  of  a  single  rigid  body  in  Newton-Euler’s  form  as  well  as  in  Lagrange’s 
form  and  establish  the  connection  between  the  two  sets  of  equations.  The  motion  of  a  single  body  with 
constraints  is  discussed  next  in  Section  3.  Using  Lagrange  multiplier  method  the  idea  of  constraint 
forces  and  torques  is  explained.  The  equations  of  motion  are  formulated  using  the  two  approaches 


*  Relative  motion  of  two  adjacent  bodies  is  a  pure  rotation  with  one,  two  or  three  degrees  of  freedom 
according  as  it  is  a  pin  joint,  universal  joint  or  ball-and-socket  joint. 
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mentioned  above.  In  Section  4  the  ideas  developed  in  Sections  2  and  3  are  extended  to  multi-body 
systems.  The  case  when  all  connections  are  ball-and-socket  joints  is  considered  first.  This  is  followed 
by  the  case  when  the  joint  may  be  universal  or  pin  joint.  Conclusions  are  presented  in  Section  5. 


2.0  RIGID  BODY  EQUATIONS  OF  MOTION 

The  equations  of  motion  of  a  rigid  body  can  be  written  down  by  applying  the  principles  of 
linear  and  angular  momentum  or  by  using  the  Lagrange’s  equation.  The  principles  of  linear  and  angular 
momentum  yield 


m 


d*rc 

dt2 


=  F 


(1) 


dLc 

dt 


=  G 


(2) 


where  rc  is  the  position  vector  of  the  centre  of  mass,  m  is  the  mass  of  the  body,  Lc  is  the  angular 
momentum  about  the  centre  of  mass,  F  is  the  sum  of  external  forces  and  G  is  the  sum  of  external 
torques  with  respect  to  the  centre  of  mass  C.  Equations  of  motion  (1)  and  (2)  are  valid  in  an  inertial 
(spatially-fixed)  frame  of  reference  OXYZ  (see  Fig.  2).  Since  Lc  =  Icu,  where  I  is  the  inertia  matrix 
about  the  centre  of  mass  and  co  is  the  absolute  angular  velocity  Equation  (2)  will  take  a  simple  form  if 
the  components  of  the  angular  momentum  are  referred  to  a  body-fixed  axes,  in  particular  to  the 
principal  axes.  (I  is  a  constant  diagonal  matrix  when  referred  to  the  principal  axes.) 


The  body-fixed  set  of  axes  are  obtained  from  the  inertial  axes  by  performing  three  successive  rotations. 
Before  the  first  rotation  the  two  set  of  axes  are  parallel.  The  first  rotation  is  made  about  the  Z-axis 
through  an  angle  \p  counterclockwise  to  obtain  an  intermediate  axes  C  x,  y,  Z.  A  new  set  of  axes  x2 , 
y, ,  z2  is  now  obtained  by  rotating  the  axes  X! ,  yj ,  Z  by  an  angle  6  counterclockwise  about  the  y,  -axis. 
The  final  rotation  is  carried  out  about  the  x2  -axis  through  an  angle  <p  to  obtain  the  body-fixed  axes 
x,y,z.  The  transformation  matrix  R  connecting  the  body-fixed  axes  to  the  inertial  axes  is  given  by 
(see,  for  example,  Refs.  4,  5) 


where 


Rn  =  cos^  cos#,  R12  =  cos0  sin#  sin0  -  sin0  cos0 

R j3  =  cos0  sin#  cos0  +  sin0sin0,  R22  =  sin  0  cos# 

R22  =  sirup  sind  sirup  +  cos\p  cosip,  R23  =  sin0  sin#  cos <p  -  cos \p  sirup 

R3,  =  -sin#,R32  =  cos#  sin0,  R33  =  cos#  cos <p. 

The  three  angles  ip,6,  <p  specifying  the  orientation  of  the  body  are  called  Euler’s  angles.  In  terms  of  the 
Euler  angle  rates  0,  #,  0  (dot  denotes  derivatives  with  respect  to  time)  the  components  cdx ,  cjy ,  cdz  of 
the  angular  velocity  cd  expressed  in  body-fixed  axes  are  given  by 

cox  =  0  -  ip  sin# 

ojy  =  0  cosd  sin0  +  #  cos <p  (4) 

coz  =  i p  cosd  cos <p  -  6  sin0. 


Let  the  body-fixed  axes  be  in  the  direction  of  principal  axes  of  inertia  and  let  Ix ,  Iy ,  Iz  denote 

d 

the  moments  of  inertia  about  three  axes.  Remembering  that  the  inertial  derivative  —  (ltd)  =  ltd  + 

dt 

to  x  ltd  in  the  body-fixed  axes,  Equation  (2)  becomes 


Ix  wx  -  (Iy  -  Iz)cdywz  =  Gx 

Iy  «y  -  (Iz  -  Ix)tdx<dz  =  Gy  (5) 

h  « z  ~  Ox  "  Iy)^xwy  =  Gz. 


These  are  Euler’s  equations  of  motion  for  a  single  rigid  body.  Euler’s  Equation  (5)  together  with  Equa¬ 
tion  (4)  and  Newton’s  Equation  (1)  yield  six  second-order  differential  equations  for  the  determination 
of  xc,yc  zc,  0,0  and  \p. 

We  mention  that  from  computational  point  of  view  it  may  be  convenient  to  solve  Equation  (5) 
and  Equation  (4)  when  inverted  as  first-order  equations  in  wx ,  cdy ,  wz ,  0,  8  and  \p .  Solving  Equation  (4) 
for  0, 0 ,  0  we  obtain 


0  *  <ox  +  tan#  (uy  sin0  +  tdz  cos0) 

#  =  cdy  cos 0  -  cdz  sin0  (6) 

0  =  — — -  (tdy  sin0  +  cdz  cos0) 
cos 8  ’ 


for  8  =£  x/2. 


Let  us  now  determine  the  equations  of  motion  using  Lagrange’s  equation 


d  / 3T\  3T 

-Q‘;i-1’2 . ••  «7» 

where  T  denotes  the  kinetic  energy,  q(  the  ith  generalized  co-ordinate,  the  generalized  velocity,  and 
Qj  the  ith  generalized  force.  Assuming  again  the  body-fixed  axes  in  the  direction  of  the  principal  axes 
we  have 


T  = 


GJV 


+  Iy  COy  +  Iz 


(8) 


with  xc,  yc,  zc,  0,  0,  0  as  the  six  generalized  co-ordinates.  Substituting  Equation  (8)  into  Equation  (7) 
we  obtain  the  Newton’s  Equation  (1)  for  the  centre  of  mass  from  the  first  three  equations  with  xc,  yc, 
zc  as  generalized  co-ordinates: 


where  F, ,  F2 , 
0, 0,  0  we  get 


d2x 


c  d2yc 


d2  z„ 


m 


=  Fj,  m 


=  F2,  m 


=  Fi 


(9) 


dt2  "  dt2  **’  dt2 
F3  are  the  components  of  F  expressed  in  inertial  frame.  For  the  other  three  co-ordinates 


P* 


3T 

-  Gy”  ^z)wy ^z 


3T 


3T 


Pa  =  — X-  =  Iy  ojy  cos 0-  Izojz  sin0,  —  =  -  IXCJX0  cos0  -  I  coy0  sin0  sin0-  Izo>z0  sin0  cos 0 
ou  oo  77 

_  3T  3T 

P *  =  +  *ywy  COS0  sin0  +  h^z  COS0  C°S0.  “  =  0 


(10) 


30 


30 


where  p^,  pe  and  p^  denote  the  generalized  momenta.  Thus  the  Lagrange’s  equation  corresponding  to 
the  co-ordinates  0,0,0  can  be  written  as 

P0  ~  (ly-Iz)^y^z  =  G, 

Pa  +  (wy  sin0  +  cjz  cos0)  (Ixo)x  +  tan0  sin0  Iy  cuy  +  tan0  cos0  Iza>z)  -  G2  (11) 

P*  =  G3 

since 

Ixwx  0  cos0  +  (IytPy  sin0  +  Izwz  cos0)  0  sin0 
=  Ixcjx(ojysin0  +  ojz  cos0)  +  (Iy  uy  sin0  +  lzu>z  cos0)(o>y  sin0  +  cjz  cos0)tan0 

from  Equation  (6).  Equations  defining  p0,  p9 ,  can  be  inverted  to  give 
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=  P*/Ix 


1  T  sin0  „  "1 

—  jj^  (P*  +  P*  sul0)  +  Pe  cos^J 

1  f  COS 0  1 

;;  l^j  <p*+p.“8>-  p.^J- 


For  rotational  equations.  Equations  (11)  and  (6)  with  cox ,  <oy ,  u>z  defined  by  Equation  (12)  may 
therefore  be  used  instead  of  Euler’s  Equation  (5)  and  Equation  (6).  By  comparing  Equations  (5)  and 
(11)  it  is  easily  seen  (note  the  expressions  for  p^ ,  pe ,  p^  )  that 

G,  =  Gx 

G2  =  Gy  cos0  -  Gz  sin0  (13 

G3  =  -Gx  sin#  +  cos0  (Gy  sin0  +  Gz  cos0). 

By  inverting  Equation  (13)  we  get  Gx ,  Gy ,  Gz  in  terms  of  Gi ,  G2 ,  G3  : 

Gx  =  G, 

sin0 

Gy  =  G2  cos 0  + — —  (G!  sinfl  +G3)  (14 


wow 

Gz  =  -G2  sin0  + - -  (G,  sin0  +  G3 ). 


3.0  RIGID  BODY  WITH  CONSTRAINTS 

Before  considering  the  multi-body  dynamics  let  us  first  consider  the  case  of  a  single  body  with 
constraints.  The  constraints  reduce  the  number  of  degrees  of  freedom.  For  example,  if  a  point  of  the 
body  is  fixed  the  motion  is  a  pure  rotation,  called  gyroscopic  motion,  and  has  three  degrees  of  freedom. 
For  a  pin  joint  we  have  only  one  degree  of  freedom  and  so  on  for  other  types  of  joints. 

3.1  Gyroscopic  Motion 

Let  us  assume  that  a  point,  say  H,  of  the  body  is  fixed.  Let  CH  =  c  be  expressed  in  body- 
fixed  axes.  From  Figure  2  we  can  write  the  constraint  relation  as: 

rH  =  rc  +  Rc  =  a  (15) 

where  a  is  a  constant  vector.  Equation  (15)  is  a  vector  equation  and  is  equivalent  to  three  scalar  equa¬ 
tions.  Due  to  constraint  Equation  (15)  there  is  a  constraint  force  fH  acting  on  the  body  at  the  hinge 
point  H.  The  Equations  of  motion  (1)  and  (2)  must  be  replaced  by 

d2rc 

m  =  F  +  fH  (16) 

Iw  +  u  x  Iu  =  G  +  c  x  RTf||  (17) 

where  it  is  assumed  that  fI(  is  expressed  in  the  inertial  frame  and  the  superscript  T  denotes  the  trans¬ 
pose  of  a  matrix.  Since  constraint  force  fH  is  unknown,  Equations  (16)  and  (17)  cannot  be  solved. 
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There  are  now  two  ways  to  proceed.  Either  eliminate  the  constraint  force  fH  and  reduce  the  number 
of  equations  to  three  or  approximate  the  unknown  force  fH . 

The  usual  procedure  to  eliminate  the  constraint  force  fH  is  to  differentiate  twice  the  con¬ 
straint  Equation  (15): 


d% 

dt2 


R(u>  x  c  +  cj  x  (co  x  c)  =  0. 


Eliminating 


d2rc 

dt2 


from  Equations  (16)  and  (18)  we  obtain 


(18) 


fH  =  -  mR(d)  x  c  +  cj  x  (cj  x  c))  -  F. 
Substituting  Equation  (19)  into  the  rotational  Equation  (17)  we  obtain 

loo  +  cj  x  Ico  =  G  -  me  x  (d)  x  c  +  co  x  (co  x  c))  -  c  x  RT  F 


(19) 


(20) 


where  we  have  used  the  fact  that  R'1  =  RT.  Since 


c  x  (d;  x  c  +  co  x  (co  x  c))  =  ((c‘c)E  -  ccT  )co  +  co  x  ((c  •  c)E  -  ccT  )co, 
Equation  (20)  can  be  rewritten  as 


IH  d)  +  coxIHu)  =  G-  cxRTF 


(21) 


where  IH  ,  the  inertia  matrix  about  fixed  point  H,  is  given  by 

IH  =  I  +  m  ((cc)E  -  ccT), 


(c*c)  is  the  dot  product  cT  c  and  E  is  the  unit  3x3  matrix.  Equation  (21)  is  the  desired  rotational 
equation  representing  the  three  degrees  of  freedom  which  can  be  solved  to  determine  the  orientation 
of  the  body.  Having  solved  this,  the  constraint  force  fH  can  be  determined  from  Equation  (19). 


We  mention  that  Equation  (21)  can  be  obtained  directly  by  applying  the  principle  of  angular 
momentum  about  the  point  H.  However,  we  would  not  obtain  the  hinge  force  that  may  be  required  to 
monitor  the  stress  on  the  joint. 

In  writing  down  the  modified  Equations  of  motion  (16)  and  (17)  it  was  assumed  that  the 
constraint  Equation  (15)  gives  arise  to  the  constraint  force  fH,  This  can  also  be  obtained  by  using  the 
Lagrange  multiplier  method  (Refs.  5,  6).  The  kinetic  energy  T  for  the  Lagrange  method  is  augmented 
by  the  introduction  of  Lagrange  multipliers  X! ,  X2 .  X3 : 


L  =  T  +  X[  (xc  +  (Rc),  -  at )  +  X2  (yc  +  (Rc)2  -  a2 )  +  X3  (zc  +  (Rc)3  -  a3 )  (22) 

where  T  is  defined  as  in  Equation  (8)  and  (Rc)*,  Oj,  i  =  1,  2,  3  denote  the  components  of  the  vectors 
Rc,  a  in  the  inertial  frame.  The  equation 


d_ 

dt 


3L 

3xc 


=  Q. 


m 


d2xc 

dt7" 


=  F, 


+  X,. 


yields 
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Therefore  X,  can  be  identified  with  the  first  component  of  fH.  Similarly  X2  and  X3  can  be  equated  to 
the  second  and  the  third  components  of  fH  respectively.  To  see  that  the  torque  due  to  constraint 
force  fH  is  given  by  c  x  RT  fH  we  compute  the  contribution  of  augmented  terms  to  the  Lagrange  equa¬ 
tions.  Denoting  these  terms  by  gH  j ,  gH  2 ,  gn3  we  have 


T  3  T  3 

Shi  =  X  ^  (Rc)>  Shi  =  X  —  (Rc)>  gH3 


XT  —  (Rc) 

oy 


where  XT  =  (X,  X2  X3).  Since  R  =  R  co  (Ref.  4),  where 

-  f 

CO  =  I  CJ3 

V«2 

it  follows  that 


-w3 

0 

cu, 


3R 

30 


sirup  cos <p' 


=  R  I  ~sin0  0 

•  cos <p  0 


0  -COS0COS0  cos0sin$\ 

3R  /  \ 

—  =  R  I  cos0cos0  0  sin0  1 

**  1  / 

^-cos0sin0  -sin0  0  J 


Let  gHx >  8ny  >  Shi  denote  the  components  of  gH 

=  c  x  Rt 

fH  =  c  Rt  f j 

Sh*  =  (° 

_c3 

c2)RTfH 

gHy  =  (c3 

0 

-Ci  )R^  fjj 

Shz  =  (~c2 

Cl 

0)RTfH. 

Evaluating  gH  (  we  have 


0  0\ 


gHl  =  XT  T7  (Rc)  =  XT  c  =  cT  )  X  =  cT  0  0  1  RtX  =  (0  -c3  c2)RtX  =  gH, 

0<p  0<P  \0<P  / 


0-10 
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In  a  similar  way  we  can  show  that 

8h2  =  8h y  cos^  -  gHz  sin <j> 

«H3  =  -«Hx  sin0  +  cos0  («Hy  sin4>  +  Khz  COS0) 

and  this  proves  the  assertion.  We  note  that  the  torques  gH  l  >  8h2  »  8h3  would  be  needed  if  the  Lagrange’s 
equations  are  used  instead  of  Euler’s  equations. 

Rather  than  eliminating  the  constraint  force  fH  using  Equation  (19)  we  may  define  it  approxi¬ 
mately  and  solve  the  six  second-order  Equations  (16)  and  (17).  For  this  we  replace  the  Lagrange  multi¬ 
plier  terms  in  Equation  (22)  by  a  potential  function  V : 

L  =  T  -  V  (23) 


where  V  is  given  by 


Another  way  to  approximate  the  constraint  force  is  to  define  the  dissipation  function 


(25) 


where  K* ,  Ky ,  Kz  are  large  positive  constants  and  determine  the  components  of  the  constraint  force 
from  the  relations: 


*Hx 

3f 

1 

3xh 

-Kx 

fHy 

3f 

i 

Wh 

~Ky 

Vh 

3f 

r 

fHz 

3zh 

-*z 

z„, 

W'*-**'  - —  -  —  .  •- ■  «* '1  £«.i  ijuii  i  ■  — m  v-.i-  *  .'WP 

•ilf  . .  _ _ _ 


In  this  way  the  constraint  force  is  approximated  by  the  viscous  damping  force.  Of  course,  the  con¬ 
straint  force  may  be  determined  by  combining  the  elastic  and  damping  force  or  by  defining  V  and  f  in 
other  ways  than  that  done  in  Equations  (24)  and  (25). 

Rather  than  approximating  the  constraint  force  analytically  through  the  use  of  penalty 
functions  such  as  V  and  f  we  may  approximate  the  constraint  force  numerically  (Ref.  3)  by  solving  the 
equations  of  motion  and  constraint  equation  simultaneously.  To  do  this  the  equations  are  first  written 
in  the  form 


F  (u,  u,  t)  =  0.  (26) 

Now  given  un  =  u(tn)  it  is  required  to  find  un+1  =  u(tn+1)  where  tn  =  nh  and  h  is  the  time  step.  Using 
for  example  the  backward  Euler  method  Equation  (26)  becomes 


F 


0 


(27) 


which  is  solved  for  un+1  using  Newton-Raphson  method  and  sparse  matrix  techniques. 

This  method  is  not  considered  further  and  is  mentioned  here  only  for  the  sake  of  complete¬ 
ness. 


3.2  Body  with  Universal  Joint 

Let  us  now  consider  the  motion  of  a  rigid  body  connected  to  a  fixed  body  with  a  universal 
joint.  The  rigid  body  has  now  two  rotational  degrees  of  freedom.  For  definiteness  sake  let 

<t>  =  0.  (28) 

Due  to  this  constraint  there  will  be  a  constraint  torque  g,,  and  Equation  (17)  must  be  replaced  by 

Icj  +  to  x  Icj  =  G  +  gH  +cxRTfH.  (29) 

In  the  body-fixed  axes  the  unit  axes  of  rotation  are  Pi  =  (-sinf?  0  cos 0)T  and  p2  =  (0  1  0)T. 
Since  the  constraint  torque  gH  ,  by  definition,  is  perpendicular  to  both  these  axes  of  rotation  we  have 

gHz  cos 6  -  gHx  sind?  =  0  (30) 

gHy  =  0.  (31) 

Equations  (30)  and  (31)  can  also  be  obtained  by  the  Lagrange  multiplier  method.  For  this  let 

L,  =  L  +  X4  <t>  (32) 

where  L  is  given  by  Equation  (22).  Due  to  the  extra  term  X40  we  have  the  constraint  torque  gH  such 
that 


8hi  =  ^4.  8h2  “  °>  8h3  “  °- 


(33) 


Using  Equation  (14)  with  <j>  =  0  we  obtain 


«Hx  =  *4 


*Hy  =  0 


gHz  =  X4  tan0. 


(34) 
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*V  » 


Eliminating  X4  from  Equation  (34)  we  have  gHz  cosd  -  gHx  sin0  =  0  and  GHy  =  0. 

To  obtain  the  equations  of  motion  with  the  least  number  we  must  eliminate  fH  and  gH  from 
Equation  (29).  Eliminating  fH  (see  Eq.  (21))  we  have 

IH  u  +  co  x  IH  oj  =  G  +  gH  -  c  x  RtF.  (35) 

To  eliminate  gH  we  premultiply  Equation  (35)  by  the  row  matrices  p|  =  (-sin0  0  cos0)  and 

pi  -  (0  1  0): 


p^  (IH  co  +  co  x  IH co)  =  pj"  (G-cx  Rt F)  +  p}  gH 

p]  (IH  co  +  co  x  IH  co)  =  pj  (G  -  c  x  Rt  F)  +  pj  gH 

From  Equations  (30)  and  (31)  it  follows  that  p^  gH  =0  and  pj  g„  =  0.  Hence  the  two  rotational 
equations  are 

p,T  (IH  co  +  co  x  IH  co)  =  p  J  (G~  c  x  RtF)  (36) 

pT  (IH  co  +  co  x  IH  co)  =  pT  (G-cx  Rt  F)  (37) 

where  co  (Eq.  (4)  with  <t>  =  0)  is  given  by 


co 


Instead  of  eliminating  the  constraint  torque  gH  and  constraint  force  fH  from  Equation  (29) 
(or  gH  from  Eq.  (35))  we  may  determine  them  approximately  and  solve  the  six  second-order  Equa¬ 
tions  (16)  and  (29)  (or  three  Eq.  (35)).  For  this  we  can  use  the  same  procedures  as  discussed  in 
Section  3.1.  For  example,  the  term  \A<p  in  Equation  (32)  may  be  replaced  by  the  negative  of  a  potential 
function  V. 


Let  V  =  —  K where  is  a  large  positive  constant.  Then 
2 

dV  dV  dV 

gH1  =  "  30  =  "K*0,  gH2  =  "  90  =  °’  gH3  =  "  3*  =  °‘  (38) 

Using  Equations  (14)  and  (38)  and  assuming  <p  small  we  get 


gHx  =  -K**,  gHy  =  0,  gHz  =  -K+t  tand.  (39) 

We  now  show  that  this  obvious  choice  of  V  is  not  good  enough.  From  Equation  (30)  we  see 
that  gHx  =0  for  0  =  ir/2.  However,  gHx  as  defined  in  Equation  (39)  does  not  tend  to  zero  as 
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6  -*■  it/2  (unless  <p  =  0).  Moreover  gHz  becomes  infinite  as  6  -*  n/2.  To  correct  this  situation  we  set 
gHx  =  "(K^cos d)<t>,  gHy  =0,  gHz  =  -(K^s ind)<t>  (40) 

which  is  obtained  by  defining  V  =  —  (K0cos0)02. 

2 

We  remark  that  since  fH  is  defined  in  terms  of  rH  or  its  derivative  it  may  be  computationally 
advantageous  to  replace  Equation  (16)  by  its  equivalent  form 

d2  rH 

m  ~~2  -  =  F  +  mR(co  x  c  +  cj  x  (to  x  c))  +  fH  (41) 

d2rc 

which  is  obtained  by  substituting  the  value  of  - from  Equation  (18)  into  Equation  (16). 


4.0  MULTI-BODY  EQUATIONS  OF  MOTION 

In  this  section  we  derive  the  equations  of  motion  of  a  system  of  n  rigid  bodies  connected 
together  by  hinges  in  an  open  chain  such  that  each  body  is  connected  directly  to  at  most  two  rigid 
bodies. 

4.1  Systems  with  Ball-and-Socket  Joints 

First  we  consider  the  case  when  all  hinges  are  ball-and-socket  joints.  The  relative  motion  of 
any  two  neighbouring  bodies  is  therefore  a  pure  rotation  with  three  degrees  of  freedom.  Consider  three 
neighbouring  bodies  i-1,  i  and  i+1  in  the  chain  as  shown  in  Figure  3.  Let  CjHj.j  *  Lu_, ,  CjHj  *  Ljj+1 . 


(43) 
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where  and  fji+1  are  the  constraint  forces  on  body  i  at  the  hinges  H(_  j  and  H^,  F'  is  the  sum  of 

external  forces  acting  on  body  i;  G‘  is  the  sum  of  external  torques;  mj,  If  and  respectively  are  the 
mass,  inertia  matrix  and  the  absolute  angular  velocity  of  body  i;  r4  =  OCj  and  R‘  is  the  transformation 
matrix  from  body  i  to  the  inertial  frame.  By  summing  the  translational  equations  from  i  =  1  to  n  we 
obtain  the  equation  of  motion  of  the  centre  of  mass  of  the  system: 

d2r  n 

m  — ;  =  £  F1  (44) 

dt2  i=l 


where 


m  =  E  m:  is  the  total  system  mass,  (45) 

i=l 

1  n 

r  =  —  S  mjTj  is  the  system  centre  of  mass.  (46) 

m  i=l 

In  deriving  Equation  (44)  we  have  used  the  fact  that  the  constraint  force  on  body  i  at  hinge  Hi  is  equal 
and  opposite  to  the  constraint  force  on  body  i  +  1  i.e. 

fi+l.i  =  i  =  1,2, .  . ., n-1.  (47) 


Also  f1’0  =  0  =  f"-n+1. 

H  H 

The  motion  of  the  system  can  now  be  determined  from  Equations  (43)  and  (44)  provided  the  constraint 
forces  are  known. 

4.1.1  Determination  of  Constraint  Forces 

As  in  the  case  of  a  single  rigid  body  we  shall  obtain  first  the  equations  of  motion  by  eliminating 
the  constraint  forces.  The  constraint  equations  at  the  hinges  Hj,  i  =  1,2, ....  n-1  are 

ri+1  +  Ri+1  Lj+1  j  =  ii  +  R‘Liti+1 ;  i  =  1,2, . .  . ,  n-1.  (48) 


From  the  translational  equations  of  bodies  i  and  i  +  1  we  have 


d*ri 

dt2 


mi 


(F'  +  1 

H 


d2fi+l 

dt2 


1 

"Vi 


(Fi+1  +  f+1>i  +  fi+1>i+2). 
H  H 


Subtracting  Equation  (50)  from  Equation  (49)  and  using  Equation  (47)  we  obtain 


.  JL  fi-i.i  +  /-L  +  -L  )  =  d{;  i-  1,2 . n-1 

mj  h  ynij  mi+1/  mi+1  » 


(49) 


(50) 


(51) 


k-v  ■««**■’ 
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where 


dj  = 


d2fi 

dt* 


t/_l_ 

dt2  yni+1 


pi+l 


(52) 


Equation  (51)  with  f^’1  =  0  =  f^’n+1  represents  a  tridiagonal  system  and  can  be  inverted  easily  by  using 

Thomas  algorithm.  Let  A  denote  the  inverse  of  the  matrix  representing  the  tridiagonal  system  of  equa¬ 
tions.  Then 


fjf1  =  s'  aydj;  i  =  U . n-1. 

j=i  1 


(53) 


Our  next  task  is  to  express  dj  in  terms  of  Lj  i+1  and  Li+i(i.  Differentiating  the  constraint  Equation  (48) 
twice  with  respect  to  time  we  obtain 


d*ri+i 

dt1 


=  R 


i+l 


(d>i+1xLi+1>i  +  toi+1 


x  (uitIxLj4|  i)) 


-  R‘  (tij  x  Ly+1  +  ojj  x  (Wj  x  Ly+1 )).  (54) 

Substituting  Equation  (54)  into  Equation  (52)  we  obtain 


dj  R  (d>j+i  x  Li+i,j+Wj+j  x  (toi+1  x  Lj+J(j))-  R‘(tjj  x  Ly+j 

+  cjj  x  (cjj  x  Lm+1  ))  +  f—  F**1  -  —  F*  | ;  i  =  1,2 . n-1.  (55) 

yni+1  mj  J 

Thus  the  constraint  forces  can  be  determined  from  Equations  (53)  and  (55).  We  note  that  Equation  (19) 
is  a  special  case  of  Equation  (53)  with  n  =  2,  m ,  =  - ,  oj ,  =  0,  fH  =  -  fL2.  We  also  mention  that  this 
method  of  deriving  the  constraint  forces  is  different  from  that  used  in  References  1  and  2. 

4.1.2  Elimination  of  Constraint  Forces 

To  eliminate  the  constraint  forces  from  the  rotational  Equation  (43)  we  need  to  evaluate  the 
expression 


hl-t  *  (R>*T  fH  +  LU+1  *  (Ri)T^+‘ 
Substituting  Equation  (53)  into  Equation  (56)  we  obtain 


(56) 


n-1 


n-1 


^•i.i-l  x  (R  ai- 1 .  jdj  +  1  x  (R*)T  .2  ajjdj 

j=l  '  1  j»l  1  1 


(57) 


From  Equation  (55)  we  see  that  the  terms  containing  cij  are: 


(58) 


•  * 
xf.  ' 
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Li.i- 1  x  R,J  [ai-l,j  Wj  *  Lj,j+1  -  ai-ij-i"j  x  kj-i] 

-  ki+i  x  Rlj  [ay  <jj  x  Lj  j+1  -  ai>M<jj  x  Lj>MJ 

where  R'^  =  (R')T  RJ  denotes  the  transformation  matrix  from  reference  frame  j  to  i.  Using  the  identity 
Ax  (B  x  C)  =  (A  •  C)B  -  (A  •  B)C  the  first  term  in  Equation  (58)  can  be  rewritten  as 

LU-1  ><  Rij(ai-l.j*j  x  Lj>j+1)  =  a^y  [(Lm_,  •  RijLjij+1)Rijcbj-  (Lia_j  •  R«Wj)R%>j+1] 

=  ai-1,j(Lj,i_i  RiiLjJ+1Rii(bj-RiiLjtj+1LTi_1Rij<bj) 

-  ai- 1 ,j R'j (Rji Lyi- 1  Rij LJ  i+ j  E  -  Ly+1LT  i_,)R«  (Oj  (59) 

Hence  Equation  (58)  can  be  simplified  as 

-Rij  KjjRijcjj  (60) 

where  the  matrix  Ky  (ij=l, . . .  ,n)  is  given  by 

Kij  *  -Rji  [Lm-1  RU(ai-ljLj,j+l'ai-lJ-lLj,j-1) 

-Lu+lR'H^jLjj+l  "  ai,j-lLj,j-l)jE 

“  (^jkjj+l  "  “ij-lkj-l^U+l  (61) 

with  L,  o  =  0  =  Ln  n+i .  Using  the  fact  that  ay  =  ay  it  is  easy  to  see  that  Ky  =  Ky . 

Similarly  combining  the  cjj  and  Fj  terms  in  Equation  (57)  we  get 

-RijWj  X  KyWj  +  Ny  +  Uy  (62) 

where 

Nij  =  -Wj^Wj  [ai,j-i  Li,i+1  -  ai-i,j-i  L|  i_ ! )  x  R'JLj  j_, 

"Kjki+l  ~  ■i-ljl'i.i-l  )  x  R,jLjj+iJ  (63) 

uij  ”  “  "  ai-l,j-l)LU-l  ~  (^j  ”  ai,j-  i  ) 

1*1, i+l]  x  (R‘)T  Fj. 


(64) 
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Using  Equations  (60)  and  (62),  the  rotational  Equation  (43)  for  body  i,  i  =  1,2, .  . 
as 


2  RijKjjRij(jj  +  .2  x  KjjtUj  =  Gj  +  .2  (N„  +  u*) 


,n,  can  be  written 


(65) 


where 


Ky  =  Kj j ,  i  *  j 

=  li  +  Kjj.i^j. 

Equations  (44)  and  (65)  give  the  required  3n+3  scalar  equations  of  motion. 


4.1.3  Approximate  Determination  of  Constraint  Forces 

Instead  of  determining  the  constraint  forces  from  Equation  (53)  exactly  we  shall  now 

rf 

use  approximate  methods  discussed  in  Section  3.1  to  determine  these  forces.  Let 


rHi  ri  + 


*Hi  =  *!♦!  +  R,+  1Li+i.i- 


Then  the  constraint  equation  at  the  hinge  H;  (see  Eq.  (48))  can  be  written  as: 

rHi  ~  rHi  "  °- 

Differentiating  r^  twice  with  respect  to  t  we  get 


d2rm  dJrHi 


dt2  dt2 


+  Ri+1  (<oi+1  x  Li+1>i  +  coi+1  x  (wi+1  x  Li+1 


Using  Equation  (42)  we  obtain  the  following  differential  equations  for  rj^,  i  =  1,2.  . .  ,n-l: 


d2rm 


mi+l  ”  R*  1  +  mi+l  R1+1  ^i+l  x  Li+I  j  +  CUj+1  X  (u?i+1  X  Lj+i  j)^  +  +  *H  •  (®®) 


To  determine  the  constraint  forces  approximately  we  define  the  potential  function 

Vi  =  ^  Ki  (fHi  ~  rHi )  (rHi  "  rHi) 

where  Kj  is  a  large  positive  constant.  The  constraint  force  f  j^1*'  is  therefore  given  by 


(67) 


3V, 


f>U  -  -  — *  -  -K,  (4  -  r~.),  i-lA . n-1 


(68) 


Hi 


where 


rHi  =  rHi—  I  +  R1  ^Li,i+1  " 


and  r^j  is  obtained  by  solving  the  differential  equation 


dJrH1 

"i  ST  -  F‘  ♦  ">,B‘ 


x  L 


1,2 


+  OJj  X  (OJj  X 


ft.  2 


(69) 


Equations  (43),  (66)  and  (69)  with  constraint  forces  P*1-',  i  =  1,2, . . .  ,n-l  given  by  Equation  (68) 

determine  the  motion  of  the  multi-body  system.  Comparing  these  equations  with  those  in  Equation  (65) 
we  see  that  although  the  number  of  differential  equations  to  be  solved  is  more,  they  are  less  densely 
coupled  and  may  be  as  convenient  to  solve  on  a  computer  as  the  more  densely  coupled  Equation  (65). 


We  mention  that  the  constraint  forces  may  also  be  determined  by  defining  the  dissipation 


function 


fi  "  iK'i  (4  -  *Hi)‘  (*Hi  -  *Hi) 


•  —  \T  /•< 


(70) 


where  K  is  a  large  positive  constant.  The  force  f^1''  is  now  given  by 

af. 

c'' ' '  mT  "  "K‘  (i;‘ ' 


‘Hi 


4.2  Systems  with  Ball-and-Socket  Joints,  Universal  Joints  and  Pin  Joints 

We  now  consider  the  case  when  the  rotational  degrees  of  freedom  at  some  joints  may  be  less 
than  three  i.e.  the  hinges  may  be  universal  (two  degrees  of  freedom)  or  pin  (one  degree  of  freedom) 
joints.  Due  to  universal  and  pin  joints  at  the  hinges  the  equations  of  motion  (42)  and  (43)  must  be 
replaced  by 


d2r; 

m;  — -T-  =  F'  +  fi;'-1  +  P-i+1 


>  dt2 


H  K 


.  T  ,  ; 


IjWj  +  Wjx  I.u.  =  G*  +  -  Ri,i+1  gtf 1,1  +  Lj  j_,  X  (R‘)  frf-1 


T  j 


+  lm+1  *  (Ri)  4i+1- 


(71) 


(72) 


where  g' •i_  1  denotes  the  constraint  torque  on  body  i  at  the  hinge  i-1  expressed  in  the  ith  body  axes. 

H 

Of  course,  g*'°  =  0  =  g"+1,n . 

H  H 

To  determine  the  orientation  of  the  bodies  we  need  to  eliminate  not  only  the  unknown 
constraint  forces  but  also  the  unknown  torques,  The  unknown  constraint  forces  can  be  eliminated  in 
the  same  way  as  was  discussed  in  Section  4.1.  The  rotational  equations  can  therefore  be  written  as 
(see  Eq.  (65)): 


2  R^KjjR'^cOj  +  2  R^x  K^w,  *  G'  +  g^-l  -  R*-i+1  gj?1-1  +  2  (N-j  +  uJ.  (73) 

j=l  J  j=  j  J  J 1  J  n  H  j  —  |  1J  *  J 

i  =  1,2, .  . .  ,n.  Multiplying  Equation  (73)  by  R1'  and  summing  the  equations  from  i  =  1  to  n  we  obtain 
2  Ru  2  R^KijR^cb,  +  2  R1'  2  R^W;  x  =  2  Ru  (G‘  +  2  (N..  +  ufi)  J  (74) 

i=  1  j=l  1  i=  1  j=l  1  JI  3  i=l  \  j=l  U  1J/ 

since  2  R«  (gM"1  -  R*>i+1  =  gjf0  =  0. 

4.2.1  Determination  of  Constraint  Torques 

We  now  determine  the  constraint  torques  g*’1-1 ,  i  =  2,3, . . .  ,n.  For  this  we  add  all  the 
rotational  equations  except  the  first  i-1  equations.  From  Equation  (73)  we  get 

2  Rik  2  RkjK.iRkjcji  +22  R^W;  x  KiItcO;  =  2  RikGk  +  2  Rik  2  (N-.+il,) 

k  =  i  j=i  *J  1  k  =  i  j=l  ‘  Jk  J  k  =  i  k  =  i  j=i  K)  KJ 

+  i  Rik  ^gk  k-1  -  Rk-k+Ig^+1-k).  (75) 

Since  2  R,k  ^jk_l  -  Rk,k+1  g]5j+1'k^  =  g^1-1  we  have 

gii-1^  £  Rik  2  Rk^K.  .Rkj<j.  +  2  2  Rk^<n>,  x  Kjkco,  -  2  Rik  (Gk  +  2  (R.  +  u..)), 
h  k  =  i  j=i  kJ  1  k  =  i  j=l  J  J  J  k  =  i  \  j=l  kJ  k)/ 


i  =  2,3, . . .  ,n. 


4.2.2  Elimination  of  Constraint  Torques 


In  the  presence  of  universal  and  pin  joints  the  relative  angular  velocity  of  two  neighbouring 
bodies  can  be  defined  in  terms  of  fewer  angular  rates  —  two  in  the  case  of  a  universal  joint  and  one  for 
a  pin  joint.  Let  11)  denote  the  relative  angular  velocity  of  body  i  relative  to  body  i-1. 


Wj  *  Rl,i-1  +  Slj 


where 


ni  "  Pil'fu  +  Pi2  7i2  +  PjjTij  =  .2  Pjj  7jj>  i  =  2,3 . n. 


ns  =  1,2  or  3  according  as  the  hinge  is  a  pin,  universal  or  ball-and-socket  joint  and  p^  are  unit  vectors 
along  the  axes  of  rotation.  For  example  in  the  case  of  universal  joint  we  may  take  *=  yi2  =  0t. 


* 
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From  Equation  (4)  with  #  =  0  it  follows  that  pn  =  (-sinflj  0  cos0j)T  and  pi2  =  (0  1  0)T .  Using 

Equation  (77)  we  can  express  u>i  in  terms  of  u>t  and  the  relative  angular  rates  : 

ojj  =  R1,i_1  tUjj  +  Sli 

=  E,'*-i(R,-1',-2«i  _2  +  SVj)  + 

=  r*-'-2  u>._2  +  Ri,i_1  n._j  +  n. 


since  R*’1  1  R'"1’*  2  =  R‘*‘  2.  Continuing  the  above  process  we  obtain 


ujj  =  R*1  cUj 


+  2 

j=2 


Rij  flj. 


(79) 


In  the  same  way  cjj  can  be  expressed  in  terms  of  cOj  and  -y...  Differentiating  Equation  (77)  we  get 


"i 


=  Ri,i-1 


0J- 


1  +  +  Pi27i2  +  Pi3Vi3  +  V; 


(80) 


where  v;  is  given  by 


vi 


-  -n;  x  r'1'-1 


"l-I  +  Pil^i,  +  P|2*l 


■  2 


Pi37i3- 


Using  Equation  (80)  recursively  we  obtain 


"i  “  R‘ "l  +  ,?2  R,,(Pjl>jl  +  Pj27)2  +  Pj 3 7j 3 )  +  .2 


Rijv:. 


(81) 


n 

The  rotational  degrees  of  freedom  n'  =  2  nf  +  3.  Therefore,  to  determine  the  orientation 

i=2 

of  the  multi-body  system  we  require  n'  equations.  Equation  (74)  with  cjj  and  cij  defined  by  Equa¬ 
tions  (79)  and  (81)  provides  three  equations.  To  determine  the  rest  of  the  equations  we  use  Equa¬ 
tion  (76).  By  definition,  the  constraint  torque  is  perpendicular  to  the  axes  of  rotation  and  hence  we 
have: 


P?h.  *h-1  “  °*  1  =  2»3>  •••»"»  hj  =  1.  •  •  •  »ni- 


(82) 


Substituting  the  value  of  g*^-1  from  Equation  (76)  we  obtain  the  remaining  n'-3  equations: 


RkjKlfiRkjwi  +22  Rk-*c*?i  x 

KJ  1  k  =  j  j=J  J  JK  J 


_  r  n  » 

?iTh  2  R,k  2 

*  [_k  =  i  j=1 

-  j,  (G"  *  (N«J  *  “»)>)]  ’  0- 


(83) 


Substituting  the  value  of  cbj  from  Equation  (81)  into  Equations  (74)  and  (83),  the  equations  of  motion 
can  be  rewritten  in  the  following  form: 
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where 


bnu>l 

+ 

n 

2  1 
k  =  2 

^b!k  >k  1  +  •  •  • 

*  bnl  n»k) 

=  fr 

(84) 

hi  . 

bil  "l 

+ 

n 

2  I 

k  =  2 

(h/khj  7k  i  +  •  •  • 

=  f''i,i  =  2,3,...,n 

(85) 

hj=l, . . .  *ni» 

n 

n 

bu  = 

2 
i=  1 

2 

j=i 

R^KjjR*1 

(3x3  matrix) 

hhk  _ 
blk  " 

n 

2 

j=k 

n 

2 
i=  1 

R,jKijRikPkhk 

(3x1  matrix) 

n 

/  n 

\  11 

n 

fl  = 

2 
i=  1 

Ru 

(G‘  +  ,?i  (Nb 

**)  ~  l 

Rh  2  R'JtOi  xK.cj. 

j=i  J  J 

-2(22  R^K.jR'M  v.  (3x1  matrix) 

k  =  2  lj=k  i=  1  1J  f 

h.  /  h  \T 

bjj  =  fbjM  (1x3  matrix) 

blkh,  "  P,Th.  \  f  RiiKrJRrkPkh  (scalar) 

*  i  j-k  r=i  k 

*  p*,  [j,  Rik(Gk  ♦  j,  (N«  *  %»)  -  j,  I,  Rll“, * 

-  2  2  R'JK  -  2  Rrkv.l  (scalar) 

rai  j=l  IJ  k  =  2  J 


(scalar) 


n  n  J 

2  2  R,JK  .  2 

r  =  i  j=l  rj  k  =  : 


(scalar) 


and  ojj  can  be  evaluated  by  using  the  recursive  Equation  (77).  For  computing  the  cross  products 
appearing  in  the  above  equations  the  relation  axb  =  a  b  can  be  used. 

4.2.3  Approximate  Determination  of  Constraint  Torques 

In  this  section  we  shall  discuss  procedures  for  determining  approximately  the  constraint 
torques.  Assume  first  that  body  i  is  connected  to  body  i-1  by  a  pin  joint.  Let  p  denote  the  unit 
vector  along  the  axes  of  rotation  of  body  i  relative  to  body  i-1. 


Ru  1  Pj  -  P;  (8< 

where  Rl,i-1  is  the  transformation  matrix  connecting  the  components  of  a  vector  expressed  in  frame 
i-1  to  frame  i  and  is  given  by 

Ri.i-l  „  (Ri)"1  Ri-l 


(87) 
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Substituting  Equation  (87)  into  Equation  (86)  and  multiplying  both  sides  by  R1  we  obtain 

Ri_1  Pj  =  R'Pj  (88) 

The  constraint  Equation  (88)  gives  three  scalar  equations  (only  two  are  independent).  Let  the  three 
scalar  equations  be  written  as 


Eil  "  °’  Ei2  “  °>  Ei3  =  °- 

As  in  the  case  of  a  single  body  we  now  define  a  potential  function  V;  by  the  relation 

vi  -  j  Ki  K  *  ♦  e?3) 

and  determine  g1’1'1,  g1’1' *,  gj:1'1  by  the  relations 


avi 

3V. 

3V. 

_ 

ei.i-l  =  -  -  oU-1  =  _ 

H2  30.  ’  bH3 

1 

30.  ’ 

30. 

(89) 


(90) 


(91) 


Having  determined  these,  the  components  of  the  constraint  torque  in  the  body  frame  i  can  be  deter¬ 
mined  using  Equation  (14): 


g*>i- 1 
sHx 

=  e’’1-1 
bhi 

gi.i-t 

BHy 

COS0j  + 

sin0j 

(gm'  s,n0i  +  gH3_ 

gH2 

COS0j 

Gm"‘ 

Hz 

= 

6H2 

1  sin0;  + 

COS0J 

(g’n’r 1  sin0i  +  gH3 

1 

cos 0. 

(92) 


It  should  be  noted  that  these  components  are  not  required  if  the  rotational  equations  are  written  in  the 
Lagrange  form  (see  Eq.  (11)). 

We  can  also  express  the  constraint  condition  in  terms  of  the  Euler  angle  rates.  For  this  let  SX 
denote  the  relative  angular  velocity  of  body  i  with  respect  to  body  i-1.  Then  there  exist  two  unit 
vectors  qu,  qj2  orthogonal  to  each  other  such  that 

q[,  =  0,  q[2  n,  =  o  (93) 

i.e.  the  relative  angular  velocity  is  orthogonal  to  the  constraint  axes  qn  and  qj2. 

To  determine  the  constraint  torque  by  using  Equation  (93)  we  form  the  dissipation  function 

f.: 

(i  ■  \  K  ((in  *  (<fa  n,)2)  (94) 


and  obtain 
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3f. 

3fi 

3fj 

-  Dfi.i-l 

30.  ’  bH2 

=  _  ^1-  ffU-l 

30. ’  sH3 

30. 

(95) 

The  components  of  the  torque  in  frame  i  can  now  be  obtained  from  Equations  (92)  and  (95). 

Let  us  now  consider  the  case  of  a  universal  joint.  In  this  case  there  is  a  vector  q;  such  that 


qT  f2j  =  0 

The  constraint  torque  can  be  determined  as  above  by  defining  the  dissipation  function  f. : 


(96) 


(97) 


Example 


As  an  example  to  the  application  of  determining  constraint  torques  approximately  we  con¬ 
sider  the  case  of  a  remote  manipulator  system  with  six  degrees  of  freedom  —  a  universal  joint  at  the 
shoulder,  a  pin  joint  at  the  elbow  and  a  ball-and-socket  joint  at  the  wrist  (Ref.  8). 


Elbow 


FIG.  4 


Assuming  the  links  are  rigid  it  can  be  treated  as  a  three-body  system  —  upper  arm,  lower  arm  and  the 
payload.  (In  the  terminology  used  here  it  should  be  regarded  as  a  four-body  system  with  the  first  body 
held  stationary.)  Let  the  axes  of  rotations  at  the  shoulder  be  z  and  y  axes  and  that  at  the  elbow  be  y 
axis.  Expressing  =  cu,  and  q,  in  inertial  frame  and  using  constraint  Equation  (96)  we  have 


| cosi|/j  simp  j  o) 


which  yields 


cos^j cos0 j  -  (IjSint/'j' 

^jSin^jCOsdj  +  fljcosi |  =  0 
-  0,sin0, 

0jCOS0,  =  0 


(98) 


To  find  the  constraint  conditions  at  the  elbow  we  apply  Equation  (88)  with  pl^  =  (0  1  0) 
and  obtain 


A 

A 
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E21  =  cos02sin02sin02  -  sin02cos02  -  cosi/zj  sin0j  sin0j  +  sin0j  cos0(  =  0 
E22  =  sin02sin02sin02  +  cos02cos02  -  sin0, sin0j sin0,  -  cosi/zj  cos0j  =  0 
E23  =  cos02sin02  -  cos0jsin0j  =  0  (99) 


Let  us  first  determine  the  constraint  torque  at  the  shoulder.  For  this  we  form  the  dissipation 


function 


t, 


and  obtain 


3ft  3fj  3f, 

4.  -  -  ^  -  -  KV“2V  4l  -  -  -  »•  «H3  -  -  ^  -  »• 

where  the  superscript  S  stands  for  the  shoulder.  Using  Equation  (92)  we  get 

g„x  =  -  K'0jCos20,,  g^  =  -  K'0,  sin0,  cosfij  sin0j ,  g*z  =  -  K'0,  sin0j  cos0j  cos0j 

From  Equation  (98)  it  follows  that  0j  =  0  and  hence  0j  =  constant  =  0.  Therefore  for  small  0 j ,  0,  the 
torque  can  be  obtained  from  the  relations: 

Bhx  =  "  K'0,  cos2  0,,  g^y  =  0,  g®z  =  -  K'0, sin0,  cos0 , 

or  (dividing  by  cos 0 , )  from  the  equations: 

Shx  =  "  K'01  cos<9i ,  g^y  =  0,  g*z  =  -  K'0,sin0,.  (100) 

We  can  also  determine  the  torque  by  forming  the  potential  function  V,  =  —  (Kcos0,)0,.  In 
this  way  we  have  ^ 


g„x  =  -  K0,  cos0, ,  g„y  =  0,  g®z  =  -  K0,sin0, . 


(101) 


Of  course,  the  constraint  torque  can  be  determined  by  combining  Equations  (100)  and  (101). 
We  now  determine  the  constraint  torque  at  the  elbow.  For  this  we  define 


-2kK 


+  Ea  +  EL) 


(102) 


Differentiating  partially  with  regard  to  02 , 02  and  02  we  obtain 


i 


* 


23- 


k2e  = 

8H1 


k2K  = 

®H2 


e2'  = 
gH3 


9V2 

302 

av2 

00  2 

^2 

30, 


=  -  K 


r  9e2, 

L2'  >*2 

r  3Ezi 

K  E21  - 

L  afl2 
f  3E21 

K  E„  T - 

L21  *+2 


0E 


+  E 


22 


0E 


22 


+  E 


22 


30  2 

^22 

00, 


+  E 


23 


23 


00, 


] 


+  E 


23 


(103) 


0E 


+  E 


22 


0E 


22 


30, 


+  E 


23 


23 


30, 


•)P 

where  gHjl  i  =  1,2,3  denote  the  components  of  the  torque  on  body  2  at  the  elbow  joint.  The  torque 

components  on  body  1  at  the  elbow  can  be  similarly  obtained  by  differentiating  V2  partially  with  re¬ 
spect  to  0, ,  0, ,  0, . 

Since  0,  =  0  it  follows  from  Equation  (99)  that  0,  =  0  and  02  -  0,  =0.  Substituting  the 
values  of  E,,,  E2,,  E,3  from  Equation  (99)  into  the  first  equation  of  Equation  (103)  we  obtain 


a21  =  -  K 

gHi 


|^(cos02sin02sin02  -  sin02cos 0,  -  cos0 ,  sinO ,  sin0,  +  sin0,cos0,) 
(cos02sin0,cos02  +  sin0,sin02)  +  (sin02sin02sin02  +cos02cos 02 


-  sin0,  sin0,  sin0,  -  cos0,  cos0,  )  (sin02sin02cos0,  -  cos02sin 02) 
+  (cos0,sin02  -  cos0 ,  sin0, )  cos02 cos02J  . 

Using  small  angle  approximations  it  can  be  seen  that  the  above  expression  can  be  written  as 


gHi  =  K  [(^2  "  0 1  )sin0 2  +  0,  cos(02  -0,)  -  02J 


Similarly,  we  have 


g£  =  0 


gH3  K 


[(02  -  0])  +  01  sin0,  -  02  Sin02j. 

Substituting  these  values  into  Equation  (92)  we  obtain  the  components  of  the  torque  in  frame  2: 


X  =  k[(02  ~  0 1  )sin02  +  0,  cos(0 2  -  0,)-  02J 


g2t  = 

»llx 


B2'  =  0 

®lly  u 


=  ”  k[(02  ~  0 1  )cos0 2  -  0,sin(02  -  0,)J. 


(104) 


-24- 


The  components  of  the  constraint  torque  on  body  1  expressed  in  body  frame  1  can  be 
similarly  found.  These  are  given  by 

8hx  =  "  K  [(^2  ~  ^i)sinfli  +  "  02cos^2  '  6i )] 

■£  =  0  I 

8rz  =  K  -  }j/l  )cosQl  -  <t>2 sin(02  -  0,  )J. 

We  mention  that  the  components  in  Equation  (105)  can  also  be  obtained  from  the  relation 


=  -R12  e2E 

«■  gHy 


where  R12  is  the  transformation  matrix  connecting  frame  2  to  frame  1  and  can  be  taken  as: 


R12  = 


cos(02  -  0j )  0  sin(02  -  0, ) 


-sin(02-0j)  0  cos(02-01) 


Having  determined  the  constraint  torques  the  motion  of  the  manipulator  can  be  determined  by  using 
Equations  (66),  (68),  (72),  (101),  (104)  and  (105)  with  n  =  4  (n  =  3  for  Eq.  (72)). 

5.0  CONCLUSIONS 

The  principles  of  linear  and  angular  momentum  (Newton’s  and  Euler’s  law)  applied  to  each 
individual  body  in  the  chain  provide  a  simple  way  of  writing  the  equations  of  motion  of  a  multi-body 
system.  The  unknown  constraint  forces  and  torques  that  appear  in  the  equations  of  motion  can  be 
either  eliminated  or  specified  approximately  using  the  constraint  equations.  It  is  shown  that  the 
elimination  procedure  leads  to  a  densely  coupled  system  of  second-order  equations  which  can  be 
written  in  the  vector-matrix  form  B(7)  y  =  f(7,7,t)  where  B  is  an  n'  x  n'  matrix  and  n'  is  the  rotational 
degrees  of  freedom.  On  the  other  hand  the  second  method  of  specifying  approximately  the  unknown 
forces  and  torques  leads  to  a  system  of  6n  second-order  equations  where  n  is  the  number  of  rigid  bodies 
in  the  system.  The  system  of  equations  obtained  by  this  method  is  simple  and  less  densely  coupled. 
Both  these  methods  are  general  and  can  be  easily  implemented  on  the  computer. 

For  studying  the  dynamics  of  multi-body  systems  several  other  methods,  perhaps  less 
general,  have  also  been  proposed.  These  may  be  found  in  References  9-14. 
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